#include <vtkSmartPointer.h>
#include <vtkPolyData.h>
#include <vtkModifiedBSPTree.h>
#include <vtkSphereSource.h>
#include <vtkPolyDataReader.h>
#include "vtkDoubleArray.h"
#include <vtkPointData.h>
#include <vtkMath.h>
#include "vtkPolyDataWriter.h"

// HYL 10/25/13
#include <vtkVersion.h>
#include <vtkCenterOfMass.h>
#include <vtkDoubleArray.h>
#include <vtkPoints.h>
#include <vtkPolyData.h>
#include <vtkSmartPointer.h>

#include "SphericalHarmonicSpatialObject.h"
#include "SphericalHarmonicMeshSource.h"
#include "SphericalHarmonicCoefficientFileWriter.h"
#include "SphericalHarmonicCoefficientFileReader.h"

#include <limits>
#include <cmath>
#include <time.h>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <string>
#include <string.h>


/*
./ThicknessCalculator  convexHull_SPHARM8.vtk  midMyocardium_SPHARM8.vtk  walloutput_SPHARM8.vtk  midMyocardium_Thickness8.vtk
./ThicknessCalculator  convexHull_SPHARM20.vtk  midMyocardium_SPHARM20.vtk  walloutput_SPHARM20.vtk  midMyocardium_Thickness20.vtk
./Show  midMyocardium_Thickness8.vtk 512 512 Thickness
./Show  midMyocardium_Thickness20.vtk 512 512 Thickness
*/

int main(int argc, char * argv[])
{
	std::string bullseyeFile; // output regional curvature in bull's-eye format
// Load InnerMyocardium
	vtkPolyDataReader *innerReader = vtkPolyDataReader::New(); 
	innerReader->SetFileName(argv[1]);
	innerReader->Update();
	vtkSmartPointer < vtkPolyData > innerPolyData = innerReader->GetOutput();
// Create the bspTree for innerSurface
  vtkSmartPointer<vtkModifiedBSPTree> innerTree = vtkSmartPointer<vtkModifiedBSPTree>::New();
  innerTree->SetDataSet(innerReader->GetOutput());
  innerTree->BuildLocator();

// Load midMyocardium
	vtkPolyDataReader *midReader = vtkPolyDataReader::New(); 
	midReader->SetFileName(argv[2]);
	midReader->Update();
	vtkSmartPointer < vtkPolyData > midPolyData = midReader->GetOutput();

// Load OuterMyocardium
	vtkPolyDataReader *outerReader = vtkPolyDataReader::New(); 
	outerReader->SetFileName(argv[3]);
	outerReader->Update();
	vtkSmartPointer < vtkPolyData > outerPolyData = outerReader->GetOutput();
// Create the bspTree for outerSurface
  vtkSmartPointer<vtkModifiedBSPTree> outerTree = vtkSmartPointer<vtkModifiedBSPTree>::New();
  outerTree->SetDataSet(outerReader->GetOutput());
  outerTree->BuildLocator();

  bullseyeFile = argv[5];

  ///////// Get Point Normals ///////////
  vtkSmartPointer<vtkDoubleArray> pointNormalsRetrieved = vtkDoubleArray::SafeDownCast(midPolyData->GetPointData()->GetNormals());
  if(pointNormalsRetrieved)
    {
    int n_vert = pointNormalsRetrieved->GetNumberOfTuples();
    std::cout << "There are " << n_vert << " point normals." << std::endl;

double * thickness = new double[n_vert];
        vtkDoubleArray* Thickness = vtkDoubleArray::New();
        Thickness->SetName("Thickness");
        Thickness->SetArray(thickness, n_vert, 1);

  // BSPTree Inputs: pStart, pEnd
  double tolerance = .001;
 
  // BSPTree Outputs:
  double t; // Parametric coordinate of intersection (0 (corresponding to p1) to 1 (corresponding to p2))
  double innerPoint[3], outerPoint[3]; // The coordinate of the intersection
  double pcoords[3];
  int subId;
 
    for(vtkIdType i = 0; i < n_vert; i++)
      {
      double p[3], pNormal[3], pStart[3], pEnd[3], halfLength=10.0;
      midPolyData->GetPoint(i, p);
      //std::cout << "Point " << i << ": " << p[0] << " " << p[1] << " " << p[2] << std::endl;
      pointNormalsRetrieved->GetTuple(i, pNormal);
      //std::cout << "Point normal " << i << ": " << pNormal[0] << " " << pNormal[1] << " " << pNormal[2] << std::endl;
      pStart[0] = p[0]-halfLength*pNormal[0];
      pStart[1] = p[1]-halfLength*pNormal[1];
      pStart[2] = p[2]-halfLength*pNormal[2];
      //std::cout << "Start Point " << i << ": " << pStart[0] << " " << pStart[1] << " " << pStart[2] << std::endl;
      pEnd[0] = p[0]+halfLength*pNormal[0];
      pEnd[1] = p[1]+halfLength*pNormal[1];
      pEnd[2] = p[2]+halfLength*pNormal[2];
      //std::cout << "End Point " << i << ": " << pEnd[0] << " " << pEnd[1] << " " << pEnd[2] << std::endl;

	  // Note: for a typical use case (ray-triangle intersection), pcoords and subId will not be used
	  // iD is 0 if no intersections were found
	  vtkIdType iD = innerTree->IntersectWithLine(pStart, pEnd, tolerance, t, innerPoint, pcoords, subId);
	//  std::cout << "iD: " << iD << std::endl;
	//  std::cout << "t: " << t << std::endl;
	//  std::cout << "Inner Intersection Point: " << innerPoint[0] << " " << innerPoint[1] << " " << innerPoint[2] << std::endl;
	//  std::cout << "pcoords: " << pcoords[0] << " " << pcoords[1] << " " << pcoords[2] << std::endl;
	//  std::cout << "subId: " << subId << std::endl;

	  iD = outerTree->IntersectWithLine(pStart, pEnd, tolerance, t, outerPoint, pcoords, subId);
	//  std::cout << "iD: " << iD << std::endl;
	//  std::cout << "t: " << t << std::endl;
	//  std::cout << "Outer Intersection Point: " << outerPoint[0] << " " << outerPoint[1] << " " << outerPoint[2] << std::endl<< std::endl;
	//  std::cout << "pcoords: " << pcoords[0] << " " << pcoords[1] << " " << pcoords[2] << std::endl;
	//  std::cout << "subId: " << subId << std::endl;

	// Find the squared distance between the points.
	  double squaredDistance = vtkMath::Distance2BetweenPoints(innerPoint, outerPoint);

	  // Take the square root to get the Euclidean distance between the points.
	  double distance = sqrt(squaredDistance);

	  // Output the results.
	  std::cout << "Distance = " << distance << std::endl;

	  thickness[i] = distance;
      }

    // HYL
    		std::string  outFileName("dummy");
    		outFileName.erase();
        	outFileName.append(bullseyeFile);
        	std::ofstream myfile(outFileName.c_str(), std::ios::out);
        	myfile.precision(10);
        	for (int i=0; i<n_vert; i++)
        	{
        		    myfile << "thickness["<<i<<"]: " << thickness[i] << std::endl;
        	}
        	myfile.close();
    // HYL //

/*
    // HYL 10/25/13  Compute the center of mass
    vtkSmartPointer < vtkCenterOfMass > centerOfMassFilter = vtkSmartPointer< vtkCenterOfMass > ::New();
    centerOfMassFilter->SetInput(innerReader->GetOutput() );
    centerOfMassFilter->SetUseScalarsAsWeights(false);
    centerOfMassFilter->Update();

    double center1[3];
    centerOfMassFilter->GetCenter(center1);
    std::cout << "Mass center is: " << center1[0] << " " << center1[1]<< " " << center1[2] << std::endl;

	int x1Position = 0, y1Position = 0, x2Position = 0, y2Position = 0, x3Position = 0, y3Position = 0;
	bool refBasalPoint1=false, refBasalPoint3=false;

	float inSliceSpacing = 0;

//    neurolib::SphericalHarmonicMeshSource::Pointer meshsrc = neurolib::SphericalHarmonicMeshSource::New();
//    meshsrc->SetCoefs(coeflist);
//    meshsrc->SetLevel(subdivLevel);
//    meshsrc->Update();
//	double * phi_theta = meshsrc->GetPrecomputedIcosList(); // nvert*2
//	double * phi_theta = new double[n_vert];
	std::string  outFileName("dummy");

	    float split1_2 = M_PI*5/3;
        	if(refBasalPoint1||refBasalPoint3)
        	{
        		split1_2 = atan2(-x1Position*inSliceSpacing-center1[0], -y1Position*inSliceSpacing-center1[1]); // in the interval [-pi,+pi] radians
        		split1_2 = split1_2+M_PI; //in the interval [0,+2pi] radians
        	}
        	std::cout << "split1_2 is: " << split1_2*180/M_PI << " degrees." << std::endl;
        	float split2_3 = split1_2 + M_PI/3;
        	split2_3 = (split2_3<=2*M_PI) ? split2_3 : (split2_3-2*M_PI);
        	float split3_4 = split2_3 + M_PI/3;
        	split3_4 = (split3_4<=2*M_PI) ? split3_4 : (split3_4-2*M_PI);
        	float split4_5 = split3_4 + M_PI/3;
        	split4_5 = (split4_5<=2*M_PI) ? split4_5 : (split4_5-2*M_PI);
        	float split5_6 = split4_5 + M_PI/3;
        	split5_6 = (split5_6<=2*M_PI) ? split5_6 : (split5_6-2*M_PI);
        	float split6_1 = split5_6 + M_PI/3;
        	split6_1 = (split6_1<=2*M_PI) ? split6_1 : (split6_1-2*M_PI);

        	float split7_8 = split1_2;
        	if(refBasalPoint3)
        	{
            	split7_8 = atan2(-x2Position*inSliceSpacing-center1[0], -y2Position*inSliceSpacing-center1[1]); // in the interval [-pi,+pi] radians
            	split7_8 = split7_8+M_PI; //in the interval [0,+2pi] radians
        	}
        	//std::cout << "split7_8 is: " << split7_8*180/M_PI << " degrees." << std::endl;
        	float split8_9 = split7_8 + M_PI/3;
        	split8_9 = (split8_9<=2*M_PI) ? split8_9 : (split8_9-2*M_PI);
        	float split9_10 = split8_9 + M_PI/3;
        	split9_10 = (split9_10<=2*M_PI) ? split9_10 : (split9_10-2*M_PI);
        	float split10_11 = split9_10 + M_PI/3;
        	split10_11 = (split10_11<=2*M_PI) ? split10_11 : (split10_11-2*M_PI);
        	float split11_12 = split10_11 + M_PI/3;
        	split11_12 = (split11_12<=2*M_PI) ? split11_12 : (split11_12-2*M_PI);
        	float split12_7 = split11_12 + M_PI/3;
        	split12_7 = (split12_7<=2*M_PI) ? split12_7 : (split12_7-2*M_PI);

        	float split13_14 = split1_2 + M_PI/12;
        	if(refBasalPoint3)
        	{
            	split13_14 = atan2(-x3Position*inSliceSpacing-center1[0], -y3Position*inSliceSpacing-center1[1]); // in the interval [-pi,+pi] radians
            	split13_14 = split13_14+M_PI; //in the interval [0,+2pi] radians
        	}
        	//std::cout << "split13_14 is: " << split13_14*180/M_PI << " degrees." << std::endl;
        	float split14_15 = split13_14 + M_PI_2;
        	split14_15 = (split14_15<=2*M_PI) ? split14_15 : (split14_15-2*M_PI);
        	float split15_16 = split14_15 + M_PI_2;
        	split15_16 = (split15_16<=2*M_PI) ? split15_16 : (split15_16-2*M_PI);
        	float split16_13 = split15_16 + M_PI_2;
        	split16_13 = (split16_13<=2*M_PI) ? split16_13 : (split16_13-2*M_PI);


			double * phi = new double[n_vert];
			double * theta = new double[n_vert];
			double thicknessBullsEye[17];
			for(int i=0; i<17; ++i)
				thicknessBullsEye[i]=0;


	    	int nvertBullEye[17]; // number of vertex in each bull eye
	    	for(int i=0; i<17; ++i)
	    		nvertBullEye[i]=0;


	    	for( int i = 0; i < n_vert; i++ )
	    	{
	    		phi[i] = phi_theta[2 * i]; // 0~2pi
	    		theta[i] = phi_theta[2 * i + 1]; // 0~pi


	    	if( (theta[i]>=0) && (theta[i]<M_PI/3) && ( ((split6_1<split1_2)&&(phi[i]>=split6_1)&&(phi[i]<split1_2)) || ((split6_1>split1_2)&&((phi[i]>=split6_1)||(phi[i]<split1_2))) ) ) // 0~60;0~60;
	    	{
	    		thicknessBullsEye[1] += thickness[i];
	    		nvertBullEye[1]++;
	    	}
	    	else if( (theta[i]>=0) && (theta[i]<M_PI/3) && ( ((split1_2<split2_3)&&(phi[i]>=split1_2)&&(phi[i]<split2_3)) || ((split1_2>split2_3)&&((phi[i]>=split1_2)||(phi[i]<split2_3))) ) ) // 0~60;60~120;
	    	{
	    		thicknessBullsEye[2] += thickness[i];
	    		nvertBullEye[2]++;
	    	}
	    	else if( (theta[i]>=0) && (theta[i]<M_PI/3) && ( ((split2_3<split3_4)&&(phi[i]>=split2_3)&&(phi[i]<split3_4)) || ((split2_3>split3_4)&&((phi[i]>=split2_3)||(phi[i]<split3_4))) ) ) // 0~60;120~180;
	    	{
	    		thicknessBullsEye[3] += thickness[i];
	    		nvertBullEye[3]++;
	    	}
	    	else if( (theta[i]>=0) && (theta[i]<M_PI/3) && ( ((split3_4<split4_5)&&(phi[i]>=split3_4)&&(phi[i]<split4_5)) || ((split3_4>split4_5)&&((phi[i]>=split3_4)||(phi[i]<split4_5))) ) ) // 0~60;180~240;
	    	{
	    		thicknessBullsEye[4] += thickness[i];
	    		nvertBullEye[4]++;
	    	}
	    	else if( (theta[i]>=0) && (theta[i]<M_PI/3) && ( ((split4_5<split5_6)&&(phi[i]>=split4_5)&&(phi[i]<split5_6)) || ((split4_5>split5_6)&&((phi[i]>=split4_5)||(phi[i]<split5_6))) ) ) // 0~60;240~300;
	    	{
	    		thicknessBullsEye[5] += thickness[i];
	    		nvertBullEye[5]++;
	    	}
	    	else if( (theta[i]>=0) && (theta[i]<M_PI/3) && ( ((split5_6<split6_1)&&(phi[i]>=split5_6)&&(phi[i]<split6_1)) || ((split5_6>split6_1)&&((phi[i]>=split5_6)||(phi[i]<split6_1))) ) ) // 0~60;300~360;
	    	{
	    		thicknessBullsEye[6] += thickness[i];
	    		nvertBullEye[6]++;
	    	}
	    	else if( (theta[i]>=M_PI/3) && (theta[i]<M_PI*2/3) && ( ((split12_7<split7_8)&&(phi[i]>=split12_7)&&(phi[i]<split7_8)) || ((split12_7>split7_8)&&((phi[i]>=split12_7)||(phi[i]<split7_8))) ) ) // 60~120;0~60;
	    	{
	    		thicknessBullsEye[7] += thickness[i];
	    		nvertBullEye[7]++;
	    	}
	    	else if( (theta[i]>=M_PI/3) && (theta[i]<M_PI*2/3) && ( ((split7_8<split8_9)&&(phi[i]>=split7_8)&&(phi[i]<split8_9)) || ((split7_8>split8_9)&&((phi[i]>=split7_8)||(phi[i]<split8_9))) ) ) // 60~120;60~120;
	    	{
	    		thicknessBullsEye[8] += thickness[i];
	    		nvertBullEye[8]++;
	    	}
	    	else if( (theta[i]>=M_PI/3) && (theta[i]<M_PI*2/3) && ( ((split8_9<split9_10)&&(phi[i]>=split8_9)&&(phi[i]<split9_10)) || ((split8_9>split9_10)&&((phi[i]>=split8_9)||(phi[i]<split9_10))) ) ) // 60~120;120~180;
	    	{
	    		thicknessBullsEye[9] += thickness[i];
	    		nvertBullEye[9]++;
	    	}
	    	else if( (theta[i]>=M_PI/3) && (theta[i]<M_PI*2/3) && ( ((split9_10<split10_11)&&(phi[i]>=split9_10)&&(phi[i]<split10_11)) || ((split9_10>split10_11)&&((phi[i]>=split9_10)||(phi[i]<split10_11))) ) ) // 60~120;180~240;
	    	{
	    		thicknessBullsEye[10] += thickness[i];
	    		nvertBullEye[10]++;
	    	}
	    	else if( (theta[i]>=M_PI/3) && (theta[i]<M_PI*2/3) && ( ((split10_11<split11_12)&&(phi[i]>=split10_11)&&(phi[i]<split11_12)) || ((split10_11>split11_12)&&((phi[i]>=split10_11)||(phi[i]<split11_12))) ) ) // 60~120;240~300;
	    	{
	    		thicknessBullsEye[11] += thickness[i];
	    		nvertBullEye[11]++;
	    	}
	    	else if( (theta[i]>=M_PI/3) && (theta[i]<M_PI*2/3) && ( ((split11_12<split12_7)&&(phi[i]>=split11_12)&&(phi[i]<split12_7)) || ((split11_12>split12_7)&&((phi[i]>=split11_12)||(phi[i]<split12_7))) ) ) // 60~120;300~360;
	    	{
	    		thicknessBullsEye[12] += thickness[i];
	    		nvertBullEye[12]++;
	    	}
	    	else if( (theta[i]>=M_PI*2/3) && (theta[i]<=M_PI) && ( ((split16_13<split13_14)&&(phi[i]>=split16_13)&&(phi[i]<split13_14)) || ((split16_13>split13_14)&&((phi[i]>=split16_13)||(phi[i]<split13_14))) ) ) // 120~180;-15(345)~75;
	    	{
	    		thicknessBullsEye[13] += thickness[i];
	    		nvertBullEye[13]++;
	    	}
	    	else if( (theta[i]>=M_PI*2/3) && (theta[i]<=M_PI) && ( ((split13_14<split14_15)&&(phi[i]>=split13_14)&&(phi[i]<split14_15)) || ((split13_14>split14_15)&&((phi[i]>=split13_14)||(phi[i]<split14_15))) ) ) // 120~180;75~165;
	    	{
	    		thicknessBullsEye[14] += thickness[i];
	    		nvertBullEye[14]++;
	    	}
	    	else if( (theta[i]>=M_PI*2/3) && (theta[i]<=M_PI) && ( ((split14_15<split15_16)&&(phi[i]>=split14_15)&&(phi[i]<split15_16)) || ((split14_15>split15_16)&&((phi[i]>=split14_15)||(phi[i]<split15_16))) ) ) // 120~180;165~255;
	    	{
	    		thicknessBullsEye[15] += thickness[i];
	    		nvertBullEye[15]++;
	    	}
	    	else if( (theta[i]>=M_PI*2/3) && (theta[i]<=M_PI) && ( ((split15_16<split16_13)&&(phi[i]>=split15_16)&&(phi[i]<split16_13)) || ((split15_16>split16_13)&&((phi[i]>=split15_16)||(phi[i]<split16_13))) ) ) // 120~180;255~345;
	    	{
	    		thicknessBullsEye[16] += thickness[i];
	    		nvertBullEye[16]++;
	    	}
	    	else
	    	{
	    		std::cout << "theta[i] = "<< theta[i] << ";  phi[i] = "<< phi[i] << std::endl;
	    		//std::cout << "split16_13 = "<< split16_13 << ";  split13_14 = "<< split13_14 << std::endl;
	    		//std::cout << "split14_15 = "<< split14_15 << ";  split15_16 = "<< split15_16 << std::endl;
	    	}
	    	}
	    	std::cout << std::endl;

	    	outFileName.erase();
//	    	outFileName.append(bullseyeFile);
			std::ofstream efile(outFileName.c_str(), std::ios::out);
			   efile.precision(10);
			   efile << "NUMBER_OF_SEGMENTS = " << 16 << std::endl;

			for (int i=1; i<=16; ++i)
			{
				thicknessBullsEye[i] = thicknessBullsEye[i]/nvertBullEye[i];
				std::cout << "nvertBullEye["<<i<<"]: " <<nvertBullEye[i]<< ";  thicknessBullsEye["<<i<<"]: "<<thicknessBullsEye[i]<< std::endl;
				efile << thicknessBullsEye[i] << std::endl;
			}
			efile.close();

    // HYL 10/25/13 end
*/



    midPolyData->GetPointData()->SetScalars(Thickness);
    vtkPolyDataWriter *vtkwriter;
    vtkwriter = vtkPolyDataWriter::New();
    vtkwriter->SetInput(midPolyData);
    vtkwriter->SetFileName(argv[4]);
    vtkwriter->Write();

   
    }//end if(pointNormalsRetrieved)
  else
    {
    std::cout << "No point normals." << std::endl;
    }
 


  return EXIT_SUCCESS;
}
